/*********************************************************************
 * img_stab_R.h
 * ABOUT : Implementation for R-Algorithm of IS.
 * AUTHOR : Dilawar Singh
 * Written on : April 28, 2010
 * Modified on : 
 (c) Dilawar Singh, 2010
 **********************************************************************/

#ifndef _IMG_STAB_K_H_
#define _IMG_STAB_K_H_

#include <vector>
#include "stabilize_config.h"
#include "is_quaternion_K.h"
#include "is_homography_K.h"
#include "is_homography_matrix_K.h"


#include "app_engine/video_module/algo_if/af_algo_if/KalmanFilter.h"
#include "app_engine/video_module/algo_if/af_algo_if/cam_motion_mgr.h"
#include "app_engine/video_module/algo_if/af_algo_if/CamMotion.h"
#include "app_engine/video_module/video_handler/video_handler.h"

//#define MAX_SIZE 1024 
#define MAX_LENGTH 1024
#define QUAT_LENGTH 4
#define SUB_LENGTH 4
#define H_MATRIX_LENGTH 9
#define R_MATRIX_LENGTH 9
#define K_MATRIX_LENGTH 9

class img_stab_if_K : public IsHomographyK
{

private:
    cam_motion_mgr* pObjCamMotionManager;
    //CamMotion* pObjCamMotion;
    //KalmanFilter *pObjKalmanFilter;
    static video_handler* pBaseVideoHandler;

    ULONG lastVsyncTimeStamp;       // Time stamp of Vsync
    ULONG lastMotionTimeStamp;      // Time stamp of 
public:

    static int initInterface(video_handler* pVideoHandler);   
    kalmanType camInternalmatrix[9]; // camera internal matrix. 

    kalmanType arrayCamInternalMatrix[2*9]; // array for curCamera and nextCamera.
    kalmanType arrayTwoQuaternion[2*QUAT_LENGTH];  // array for current and 
    	
    /* declare the vectors to hold these values. */
    vector<kalmanType> myQuatsVec; 
    vector<kalmanType> myFilteredVec;
    
    // for s, theta and t.
    vector<kalmanType> myVecS;
    vector<kalmanType> myVecT;
    vector<kalmanType> myVecTheta;
   
public:
    /* Constructor */ 
    img_stab_if_K();
    img_stab_if_K(kalmanType *pArrayQuat, kalmanType *pArrayCamMat);

    /* Destructor */
    virtual ~img_stab_if_K();

    /*
     * This is the top most function.
     */
     void runImageStabilizeK(void);

    /* 
     * This function will give us a filtered out quaternion.
     * This function takes an array of IS_Quaternions and return low pass filtered
        Quaternions.
        
     * This is done as follow;
       Let q_klf[n] is the n'th kalman corrected quaternion and q_lpf[n] is n'th 
       low pass filtered (element of the returned array from this function) then
     * q_lpf[n] = alpha*q_klp[n] + (1-alpha)*q_lpf[n-1] 
     * It will update the lpfQuaternion and kcQuaternion member of IS_Quaternion class.
     */
    vector<kalmanType> accQuatsInQueue(vector<kalmanType>, kalmanType myQuat[4]);
	
    friend class KalmanFilter;
    friend class cam_motion_mgr;
    friend class CamMotion;

  
};
#endif

